/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: HMC6352 Compass Driver
 *         $Id: compass.c 5 2013-12-18 04:48:03Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include <string.h>
#include "maxq_config.h"
#include "i2c/maxq_i2c.h"
#include "comm/maxq_comm.h"
#include "compass.h"

/***** Definitions *****/
#define COMPASS_ADDR    (0x42 >> 1)

#define COMPASS_BUFFER_SIZE   4
#define COMPASS_BUFFER_SHIFT  2

/***** Global Data *****/
int compass_heading;
int compass_buffer[COMPASS_BUFFER_SIZE];

/****************************************************************************/
int eeprom_read(unsigned int addr, uint8_t * data)
{
  uint8_t msg[2];
  volatile uint32_t delay;

  msg[0] = 'r';
  msg[1] = addr;
  if (!i2c_master_write(COMPASS_ADDR, 2, msg))
    return 0;

  for (delay = 0; delay < 0xFFF; delay++);

  if (!i2c_master_read(COMPASS_ADDR, 1, msg))
    return 0;

  *data = msg[0];

  return 1;
}

/****************************************************************************/
int eeprom_write(unsigned int addr, uint8_t data)
{
  uint8_t msg[3];
  volatile uint32_t delay;

  msg[0] = 'w';
  msg[1] = addr;
  msg[2] = data;
  if (!i2c_master_write(COMPASS_ADDR, 3, msg))
    return 0;

  for (delay = 0; delay < 0xFFF; delay++);

  return 1;
}

/****************************************************************************/
int ram_read(unsigned int addr, uint8_t * data)
{
  uint8_t msg[2];
  volatile uint32_t delay;

  msg[0] = 'g';
  msg[1] = addr;
  if (!i2c_master_write(COMPASS_ADDR, 2, msg))
    return 0;

  for (delay = 0; delay < 0xFFF; delay++);

  if (!i2c_master_read(COMPASS_ADDR, 1, msg))
    return 0;

  *data = msg[0];

  return 1;
}

/****************************************************************************/
int opbyte_write(uint8_t data)
{
  uint8_t msg[3];
  volatile uint32_t delay;

  msg[0] = 'G';
  msg[1] = 0x74;
  msg[2] = data;
  if (!i2c_master_write(COMPASS_ADDR, 3, msg))
    return 0;

  for (delay = 0; delay < 0xFFF; delay++);

  return 1;
}

/****************************************************************************/
int compass_init(void)
{
  uint8_t data;

  compass_heading = -1;

  if (!eeprom_read(0, &data))
    return 0;

  data = 'W';
  if (!i2c_master_write(COMPASS_ADDR, 1, &data))
    return 0;

  if (!eeprom_write(6, 16))
    return 0;

  // Set compass to 20Hz, continuous
  if (!opbyte_write(0x72))
    return 0;

  data = 'A';
  if (!i2c_master_write(COMPASS_ADDR, 1, &data))
    return 0;

  return 1;
}

/****************************************************************************/
int compass_task(void)
{
  uint8_t msg[2];
  int newval;

  if (!i2c_master_read(COMPASS_ADDR, 2, msg)) {
    compass_heading = -1;
  } else {

    newval = msg[0];
    newval <<= 8;
    newval += msg[1];

    compass_heading = newval;
  }

  return 1;
}

/****************************************************************************/
int compass_cal_start(void)
{
  uint8_t data;

  data = 'C';
  if (!i2c_master_write(COMPASS_ADDR, 1, &data))
    return 0;

  return 1;
}

/****************************************************************************/
int compass_cal_end(void)
{
  uint8_t data;

  data = 'E';
  if (!i2c_master_write(COMPASS_ADDR, 1, &data))
    return 0;

  data = 'A';
  if (!i2c_master_write(COMPASS_ADDR, 1, &data))
    return 0;

  return 1;
}

/****************************************************************************/
// Convert degrees*10 to rad*2^8
int deg10_to_rads8(int deg10)
{
  return ( (((int32_t)deg10 * 3217) + 3600) / 7200 );
}


/****************************************************************************/
// Convert degrees*10 to rad*2^8
int rads8_to_deg(int rads8)
{
  return ( (((int32_t)rads8 * 720) + 1609) / 3217 );
}
